#include "../../include/device/chassisDevice.h"

C_chassisDevice::C_chassisDevice(const char *_path, int _speed)
{
    spdlog::info("C_chassisDevice init {} {}", _path, _speed);
    m_chassis_serial = NULL;
    switch (_speed)  {
    case 115200:
        m_chassis_serial = new SerialDriver(_path, BPS115200); // 串口驱动对象，包含了对串口的初始化
        break;
    case 460800:
        m_chassis_serial = new SerialDriver(_path, BPS460800); // 串口驱动对象，包含了对串口的初始化
        break;
    default:
        spdlog::error("error! SerialDriver band input wrong");
        break;
    }
    m_recvBuf = (uint8_t*)malloc(512*(sizeof(uint8_t)));
}

C_chassisDevice::~C_chassisDevice()
{
    delete m_chassis_serial;
    m_chassis_serial = NULL;
    delete m_recvBuf;
    m_recvBuf = NULL;
}

void C_chassisDevice::sandUsartData(uint8_t *buf, uint32_t bufLen)
{
    spdlog::info("sandUsartData");
    for(int i=0; i<bufLen; i++){
        printf("%c",buf[i]);
    }
    printf("\r\n");
    m_chassis_serial->Write(buf, bufLen);
}
void C_chassisDevice::sandUsartData(std::vector<uint8_t> &sandSerialBuf)
{
    m_chassis_serial->Write(sandSerialBuf);
}

//ADU除去校验码的字节数
//校验码
uint16_t C_chassisDevice::getCRC16(uint8_t *usratBuf,  uint16_t length)
{
    uint8_t i;
    uint16_t crc = 0xFFFF;
    uint8_t* ptr = usratBuf;
    //printf("crc length:%d \n", length);
    if(length == 0)
        length = 1;
    while(length--) {
        crc ^= *ptr;
        for(i = 0; i < 8; i++){
            if(crc & 1){
                crc >>= 1;
                crc ^= 0xA001;
            } else
                crc >>= 1;
        }
        *ptr++;
    }
    return crc;
}

// 对控制数据进行封包 生成协议
int32_t C_chassisDevice::produceSerialData(float lineSpeed, float angleSpeed, std::vector<uint8_t> &sandSerialBuf)
{
    uint8_t order[16] = {0};
    uint16_t crcData = 0;
    spdlog::info("produceSerialData:{},{}}\r\n", lineSpeed, angleSpeed);
    // 协议内容
    // 数据头   0x66
    // 内容长度 0xFF
    // 线速度符号位 0x00
    // 线速度数据为 0x00
    // 角速度符号位 0x00
    // 角速度数据为 0x00
    // 校验位高 0x00
    // 校验位低 0x00
    // 数据结束帧 0x0d
    // 数据结束帧 0x0a
    sandSerialBuf[0] = (0x66);
    sandSerialBuf[1] = (0x04);
    if(lineSpeed>=0){
        sandSerialBuf[2] = (0x00);
        sandSerialBuf[3] = (uint8_t) (lineSpeed*100);
    }
    else{
        sandSerialBuf[2] = (0x01);
        sandSerialBuf[3] = (uint8_t) (-lineSpeed*100);
    }
    
    if(angleSpeed>=0){
        sandSerialBuf[4] = 0x01;
        sandSerialBuf[5] = (uint8_t) (angleSpeed*100);
    }
    else{
        sandSerialBuf[4] = 0x00;
        sandSerialBuf[5] = (uint8_t) (-angleSpeed*100);
    }
    
    for (int i = 0; i < 4; i++)  {
        order[i] = sandSerialBuf[i+2];
    }
    // 对数据内容进行校验
	crcData = getCRC16(order, 4); // 将数据内容计算得到CRC
    sandSerialBuf[6] = (uint8_t)crcData;
    sandSerialBuf[7] = crcData>>8;
    sandSerialBuf[8] = 0x0d;
    sandSerialBuf[9] = 0x0a;
    // for (size_t i = 0; i < 10; i++)
    // {
    //     printf("%x ", sandSerialBuf[i]);
    // }
    // printf("\r\n");
    return 0;

}

/********************************************************************
 * @author: tianxiaohuahua
 * @date: 23.8
 * @description: processWheelSpeed 一个线程用于处理轮速数据
 * @input: void 
 * @return: void 
*******************************************************************/
int32_t C_chassisDevice::recvUsartData(std::vector<uint8_t> &recv_buffer)
{
    std::vector<uint8_t> data_buffer;
    std::vector<uint8_t> tempBuf;
    int32_t tempHeadData = 0;
    int32_t tempFindHeadFlag = 0;

    int32_t read_result = m_chassis_serial->ReadBytes(data_buffer, 0, 100);
    
    if(read_result == RECV_DONE){
        m_chassisUsartBuffer.insert(m_chassisUsartBuffer.end(), data_buffer.begin(), data_buffer.end());
    }
    /*数据帧寻找方法：
    程序开始有空的固定数据缓冲器buf，如果有接收到数据就将数据储插入到固定的缓冲区buf的最后面的位置。
    到数据帧头和帧尾的位置，将完整的数据帧保存到函数输出缓冲器中，作为函数的本次输出结果。 
    将这次找到的数据帧尾到结束的位置到这一次接收的数据的最后位置的这些剩余所有数据都转存到临时buf中，
    再将固定缓冲区中的buf内容替换成临时buf的内容，等待下一次数据*/ 
    if(m_chassisUsartBuffer.size()>0){
        // printf("read_result:%d\n", read_result);
        for (int i=0; i < m_chassisUsartBuffer.size(); i++){
            if(m_chassisUsartBuffer[i] == 0x66){ // 寻找到了包头
                tempHeadData = i;
                tempFindHeadFlag = 1;
                break;
            }
        }

        if(tempFindHeadFlag == 0)  
            return REV_NULL; // 如果在缓存区内没有找到包头，直接进入下一次数据接收等待

        for (int j = tempHeadData; j < m_chassisUsartBuffer.size(); j++){ // 开始寻找包尾 0x0A
            tempBuf.insert(tempBuf.end(), m_chassisUsartBuffer[j]); // 把数据填进buf内
            if(m_chassisUsartBuffer[j] == 0x0A){ // 检测到了包尾
                recv_buffer = tempBuf; // 输出数据
                std::vector<uint8_t> reamin_buffer; // 存放剩余数据
                reamin_buffer.insert(reamin_buffer.end(), m_chassisUsartBuffer.begin()+j, m_chassisUsartBuffer.end()); //转存剩余的数据
                m_chassisUsartBuffer = reamin_buffer; // 更新储存内存
                return RECV_DONE;
                break;
            }
        }
    }
    return REV_NULL;
}

